#include "ForwardDynamics.h"
#include "TransformEvaluator.h"
#include "RotateExpMap.h"
#include "ODESolver.h"
#include "mtxlib.h"
#undef ERROR
#include "glog/logging.h"
#include "gflags/gflags.h"
using namespace google;


ForwardDynamics::ForwardDynamics() : mMMatrix(NULL), mCVector(NULL), mModel(NULL), mTargetValueState(NULL), mTargetVelState(NULL), mFrameCount(0), mMode(TM_Prescribe)
{
#ifndef FILE_IO_OFF
	outFile.open("log.txt");
	outMatlabFile.open("matlab.m");
#endif
}
ForwardDynamics::~ForwardDynamics()
{
	clear();
}


void ForwardDynamics::SetTargetState(double* targetValue, double* targetVel)
{
	int numDof = mModel->getNumDofs();
	memcpy(mTargetValueState, targetValue, numDof * sizeof(double));
	memcpy(mTargetVelState, targetVel, numDof * sizeof(double));
}
//void ForwardDynamics::SetTarget(Model* m)
//{
//	mTarget = m;
//}

void ForwardDynamics::SetModel(Model* m)
{
	mModel = m;
	clear();
	int numLinks = m->getNumBodyLinks();
	int numDofs = m->getNumDofs();
	mExternalForce = Vecd(numDofs);
	mExternalForce = vl_0;
	mInternalForce = Vecd(numDofs);
	mInternalForce = vl_0;
	mMMatrix = new Matd(numDofs, numDofs);
	mCVector = new Vecd(numDofs);
	mTargetValueState = new double[numDofs];
	mTargetVelState = new double[numDofs];
	mBodyMMatrices.resize(numLinks);
	mBodyCVectors.resize(numLinks);
	mBodyJCMatrices.resize(numLinks);
	mBodyLastJCMatrices.resize(numLinks);
	mBodyJCDotMatrices.resize(numLinks);
	mBodyJWMatrices.resize(numLinks);
	mBodyLastJWMatrices.resize(numLinks);
	mBodyJWDotMatrices.resize(numLinks);
	mBodyLastdTdQdot.resize(numLinks);
	for (int i = 0; i < numLinks; ++i)
	{
		mBodyMMatrices[i] = new Matd(numDofs, numDofs);
		mBodyCVectors[i] = new Vecd(numDofs);
		mBodyJCMatrices[i] = new Matd(3, numDofs);
		mBodyLastJCMatrices[i] = new Matd(3, numDofs);
		mBodyJCDotMatrices[i] = new Matd(3, numDofs);
		mBodyJWMatrices[i] = new Matd(3, numDofs);
		mBodyLastJWMatrices[i] = new Matd(3, numDofs);
		mBodyJWDotMatrices[i] = new Matd(3, numDofs);
		mBodyLastdTdQdot[i] = new Vecd(numDofs);
	}
}
int ForwardDynamics::GetDim()
{
	return mModel->getNumDofs() * 2;
}

const Vecd& ForwardDynamics::GetInternalForce() const
{
	return mInternalForce;
}

void ForwardDynamics::CalculateDeriv(double* deriv)
{
	CalculateMMatrix(*mMMatrix);
	CalculateCVector(*mCVector);



	int numDofs = mModel->getNumDofs();

	Vecd generalizedForce(numDofs);
	generalizedForce = calculateForce();//calculateExternalForce();

	
	Vecd qDot(numDofs);
	for (int i = 0; i < numDofs; ++i)
	{
		qDot[i] = mModel->getDof(i)->getVelocity();
	}

	Vecd rhs(numDofs);
	rhs = generalizedForce - *mCVector;

	mQDotDot = Vecd(numDofs);
	Matd LumpedMass(numDofs, numDofs, vl_I);
	for (int i = 6; i < numDofs; ++i)
	{
		LumpedMass[i][i] *= mTimeStep * kd;

	}
	LumpedMass += (*mMMatrix);
	
	Matd invMMatrix = inv(LumpedMass);
	mQDotDot = invMMatrix * rhs;


	int j = 0;
	for (int i = 0; i < numDofs; ++i)
	{
		deriv[j++] = mModel->getDof(i)->getVelocity();
		deriv[j++] = mQDotDot[i];
	}

	dumpDerivatives(rhs, mQDotDot, generalizedForce, qDot);
	dumpMatlabFile();
}

void ForwardDynamics::dumpMatlabFile()
{
	static int ithA = 1;
#ifndef FILE_IO_OFF
	outMatlabFile << "A=" << *mMMatrix << endl;
	outMatlabFile << "lemda(" << ithA++ << ")=cond(A);"<<endl;
#endif
}

void ForwardDynamics::dumpDerivatives(const Vecd& rhs, const Vecd& qDotDot, const Vecd& generalizedForce, const Vecd& qDot)
{
#ifndef FILE_IO_OFF
	//Vec3d linearMomentum = ApproxLinearMomentum();
	//Vec3d angularMomentum = ApproxAngularMomentum();
	//outFile << "Linear Momentum:  " << linearMomentum << endl;
	//outFile << "Angular Momentum: " << angularMomentum << endl;
	outFile << "--------------" << mFrameCount << "th Frame-----------------" << endl;

	outFile << "Mass:   "<< endl;
	outFile << *mMMatrix << endl;
	outFile << "C   :   "<< endl;
	outFile << *mCVector << endl;

	outFile << "rhs:    " << rhs << endl;
	outFile << "qDotDot:" << qDotDot << endl;
	outFile << "force:  " << generalizedForce << endl;
	outFile << "qdot:   " << qDot << endl;

	Vecd q = mModel->getQ();
	outFile << "q:      " << q << endl;

#endif 
}

void ForwardDynamics::GetState(double* currentState)
{
	int numDofs = mModel->getNumDofs();
	int j = 0;
	for (int i = 0; i < numDofs; ++i)
	{
		currentState[j++] = mModel->getDof(i)->getValue();
		currentState[j++] = mModel->getDof(i)->getVelocity();
	}

}

void ForwardDynamics::SetStateAndVel(double* state, double* stateVel)
{
	int numDofs = mModel->getNumDofs();

	for (int i = 0; i < numDofs; ++i)
	{
		double q = state[i];
		double qDot = stateVel[i];
		if (abs(q) < EPSILON) q = 0;
		if (abs(qDot) < EPSILON) qDot = 0;
		mModel->getDof(i)->setValue(q);
		mModel->getDof(i)->setVelocity(qDot);
	}
}

void ForwardDynamics::SetState(double* state)
{
	int numDofs = mModel->getNumDofs();
	int j = 0;
	for (int i = 0; i < numDofs; ++i)
	{
		double q = state[j++];
		double qDot = state[j++];
		if (abs(q) < EPSILON) q = 0;
		if (abs(qDot) < EPSILON) qDot = 0;
		mModel->getDof(i)->setValue(q);
		mModel->getDof(i)->setVelocity(qDot);
	}

	bool bNeedAdjustment = false;
	for (int i = 0; i < numDofs; ++i)
	{
		Dof* q = mModel->getDof(i);
		RotateExpMap* expMap = dynamic_cast<RotateExpMap*>(q->getJoint()->getTransform(1));
		if (expMap && expMap->NeedDofAdjustment())
		{
			bNeedAdjustment = true;
			break;
			//if (expMap->AdjustDofs())

		}
	}
	if (0)
	{
#ifndef FILE_IO_OFF
		outFile << "Dof need to be adjusted" << endl;
#endif

		double k = CalculateKineticEnergy();
		int numBodies = mModel->getNumBodyLinks();
		Vecd omegaAll(3 * numBodies);
		omegaAll = vl_0;
		
		Vecd qdot = mModel->getQDot();
		for (int i = 0; i < numBodies; ++i)
		{
			calculateBodyJW(i, *(mBodyJWMatrices[i]));
			Vecd omega = *(mBodyJWMatrices[i]) * qdot;
			omegaAll[i * 3] = omega[0];
			omegaAll[i * 3 + 1] = omega[1];
			omegaAll[i * 3 + 2] = omega[2];
		}
		for (int i = 0; i < numDofs; ++i)
		{
			Dof* q = mModel->getDof(i);
			RotateExpMap* expMap = dynamic_cast<RotateExpMap*>(q->getJoint()->getTransform(1));
			if (expMap)
			{
				expMap->AdjustDofs();
			}
		}
		double testk = CalculateKineticEnergy();

		Matd newJWAll(3 * numBodies, numDofs);
		newJWAll = vl_0;
		for (int i = 0; i < numBodies; ++i)
		{
			calculateBodyJW(i, *(mBodyJWMatrices[i]));
			Matd& JW = *(mBodyJWMatrices[i]);
			for (int j = 0; j < 3; ++j)
			{
				for (int k = 0; k < numDofs; ++k)
				{
					newJWAll[i * 3 + j][k] = JW[j][k];
				}
			}
		}
		Matd newJWAllTransMulti = trans(newJWAll) * newJWAll;
		Vecd omegaAllTransMulti = trans(newJWAll) * omegaAll;
		cout << newJWAllTransMulti << endl;
		Vecd newQDot = inv(newJWAllTransMulti) * omegaAllTransMulti;
		for (int i = 0; i < numDofs; ++i)
		{
			mModel->getDof(i)->setVelocity(newQDot[i]);
		}
		double newk = CalculateKineticEnergy();

#ifndef FILE_IO_OFF
		outFile << "Dof Adjusted" << endl;
		outFile << "Original Kinetic Energy:  " << k << endl;
		outFile << "Changed Kinetic Energy :"   << testk << endl;
		outFile << "Current Kinetic Energy :  " << newk << endl;
#endif
		
	}
}

void ForwardDynamics::SetStateAndVelAsTarget()
{
	//mTargetVelState[0] = -2.5f;
	SetStateAndVel(mTargetValueState, mTargetVelState);
}

vector3 ForwardDynamics::Update(double timeStep, double* indexedEnergy, TrackMode mode)
{
	mTimeStep = timeStep;
	mMode = mode;

	if (mode == TM_Prescribe)
	{
		ks = 2500.0;
		//ks = 1000.0;
		kd = mTimeStep * ks;
	}
	else
	{
		ks = kd = 0;
	}


	ODESolver solver(ST_ExplicitEuler, this);
	solver.Solve(timeStep);
	dumpDeviationFromPerfect();

	int numDofs = mModel->getNumDofs();
	Vecd realInternalForce = *mMMatrix * mQDotDot + *mCVector;
	//double energyConsumed = 0;
	vector3 energyConsumed;

	int dofToBodies[] = { 0, 0, 0, 0, 0, 0, 1, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 4 };
	for (int i = 0; i < numDofs; ++i)
	{
		if(realInternalForce[i] > 10000.0)
			printf("%dth Force: %f, %f\n", i, realInternalForce[i], mModel->getDof(i)->getVelocity());
		energyConsumed.x += abs(realInternalForce[i] * mModel->getDof(i)->getVelocity() * timeStep);
		energyConsumed.y += abs(realInternalForce[i] * timeStep);
		energyConsumed.z += realInternalForce[i] * realInternalForce[i] * timeStep;

		indexedEnergy[dofToBodies[i]] += abs(realInternalForce[i] * mModel->getDof(i)->getVelocity() * timeStep);
	}
	
	mFrameCount++;
	return energyConsumed;

}

void ForwardDynamics::dumpDeviationFromPerfect()
{
	//Vec3d linearMomentum = ApproxLinearMomentum();
	//Vec3d angularMomentum = ApproxAngularMomentum();
	//outFile << "Linear Momentum:  " << linearMomentum << endl;
	//outFile << "Angular Momentum: " << angularMomentum << endl;

	//Vecd dTdQdotdt = ApproxdTdQdotdt();
	//Vecd dTdQ = ApproxdTdQ();
	//Vecd generalizedForce = calculateForce();
	//Vecd result = dTdQdotdt - dTdQ - generalizedForce;
	//outFile << "deviate: " << result << endl;
	//if (len(result) > 2)
	//	outFile << "Caution: large deviation\n" << endl;

}

void ForwardDynamics::CalculateMMatrix(Matd& mat)
{
	int numLinks = mModel->getNumBodyLinks();
	mat.MakeZero(); 

	for (int i = 0; i < numLinks; ++i)
	{
		calculateBodyM(i, *(mBodyMMatrices[i]));
		mat += *(mBodyMMatrices[i]);
	}

}
void ForwardDynamics::CalculateCVector(Vecd& mat)
{
	int numLinks = mModel->getNumBodyLinks();
	mat.MakeZero();

	for (int i = 0; i < numLinks; ++i)
	{
		calculateBodyC(i, *(mBodyCVectors[i]));
		mat += *(mBodyCVectors[i]);
	}

}

void ForwardDynamics::calculateBodyM(int index, Matd& mat)
{
	BodyLink* b = mModel->getBodyLink(index);
	double mass = b->getMass();

	Mat3d inertia = *(b->getInertia());
	*(mBodyLastJCMatrices[index]) = *(mBodyJCMatrices[index]);
	*(mBodyLastJWMatrices[index]) = *(mBodyJWMatrices[index]);
	calculateBodyJC(index, *(mBodyJCMatrices[index]));
	calculateBodyJW(index, *(mBodyJWMatrices[index]));

	//Matd test(mat);
	//approxBodyJC(index, test);
	//cout << *(mBodyJCMatrices[index]) << endl;
	//cout << test << endl;

	int numDofs = mModel->getNumDofs();

	Matd JC = *(mBodyJCMatrices[index]);
	Matd JW = *(mBodyJWMatrices[index]);


	Matd JCTrans = trans(JC);
	Matd JWTrans = trans(JW);

	
	Matd MassTerm = mass * JCTrans * JC;
	Matd InertiaTerm = JWTrans * inertia * JW;

	mat = MassTerm + InertiaTerm;

}
void ForwardDynamics::calculateBodyC(int index, Vecd& mat)
{
	BodyLink* b = mModel->getBodyLink(index);
	double mass = b->getMass();
	Mat3d inertia = *(b->getInertia());

	int numDofs = mModel->getNumDofs();

	calculateBodyJCDot(index, *(mBodyJCDotMatrices[index]));
	calculateBodyJWDot(index, *(mBodyJWDotMatrices[index]));

	Matd JC = *(mBodyJCMatrices[index]);
	Matd JW = *(mBodyJWMatrices[index]);
	Matd JCDot = *(mBodyJCDotMatrices[index]);
	Matd JWDot = *(mBodyJWDotMatrices[index]);


	Matd JCTrans = trans(JC);
	Matd JWTrans = trans(JW);
	Matd JCDotTrans = trans(JCDot);
	Matd JWDotTrans = trans(JWDot);

	Matd massTerm = mass * (JCDotTrans * JC + JCTrans * JCDot);
	Matd inertiaTerm = (JWDotTrans * inertia * JW + JWTrans * inertia * JWDot);
	Vecd qDot = mModel->getQDot();
	Matd termFromedTdqdt = massTerm + inertiaTerm;
	mat = termFromedTdqdt * qDot;

	for (int i = 0; i < numDofs; ++i)
	{
		Matd JCqi(3, numDofs);
		Matd JWqi(3, numDofs);
		Dof* q = mModel->getDof(i);
		calculateBodyJCq(index, q, JCqi);
		calculateBodyJWq(index, q, JWqi);

		double dTdq = 0.5 * (dot(qDot, mass * (trans(JCqi) * JC + JCTrans * JCqi) * qDot)
			+ dot(qDot, (trans(JWqi) * inertia * JW + JWTrans * inertia * JWqi) * qDot));

		mat[i] -= dTdq;

	}

}
void ForwardDynamics::calculateBodyJW(int index, Matd& mat)
{
	mat.MakeZero();

	BodyLink* b = mModel->getBodyLink(index);
	Mat4d transform = b->getWorldTransform();
	Mat3d R = ExtractRotationFromTransform(transform);
	Mat3d transR = trans(R);

	int numDof = static_cast<int>(mModel->getNumDofs());
	for (int i = 0; i < numDof; ++i)
	{
		Dof* q = mModel->getDof(i);

		if (b->dependsOnDof[q->getModelID()])
		{
			Mat4d identity = vl_I;
			Mat4d dWdq = b->getFirstDerivative(&identity, q);
			Mat3d dRdq = ExtractRotationFromTransform(dWdq);
			Mat3d omegaSkewSymmetric = dRdq * transR;
			Vec3d omega = FromSkewSymmetric(omegaSkewSymmetric);
			omega = transR * omega;

			mat[0][i] = omega[0];
			mat[1][i] = omega[1];
			mat[2][i] = omega[2];

		}

	}
}

void ForwardDynamics::calculateBodyJWq(int index, Dof* q, Matd& mat)
{
	mat.MakeZero();

	BodyLink* b = mModel->getBodyLink(index);
	Mat4d transform = b->getWorldTransform();
	Mat3d R = ExtractRotationFromTransform(transform);
	Mat3d transR = trans(R);

	int numDof = static_cast<int>(mModel->getNumDofs());
	for (int i = 0; i < numDof; ++i)
	{
		Dof* q1 = mModel->getDof(i);
		Dof* q2 = q;

		Mat4d identity = vl_I;
		Mat4d dWdq1 = b->getFirstDerivative(&identity, q1);
		Mat3d dRdq1 = ExtractRotationFromTransform(dWdq1);
		Mat4d dWdq2 = b->getFirstDerivative(&identity, q2);
		Mat3d dRdq2 = ExtractRotationFromTransform(dWdq2);
		Mat4d dWdq1q2 = b->getSecondDerivative(&identity, q1, q2);
		Mat3d dRdq1q2 = ExtractRotationFromTransform(dWdq1q2);

		Mat3d omegaSkewSymmetric1 = dRdq1 * transR;
		Vec3d omega1 = FromSkewSymmetric(omegaSkewSymmetric1);
		Mat3d omegaSkewSymmetric2 = dRdq1q2 * transR + dRdq1 * trans(dRdq2);
		Vec3d omega2 = FromSkewSymmetric(omegaSkewSymmetric2);
		Vec3d omega = trans(dRdq2) * omega1 + transR * omega2;

		mat[0][i] = omega[0];
		mat[1][i] = omega[1];
		mat[2][i] = omega[2];



	}
}

void ForwardDynamics::calculateBodyJCq(int index, Dof* q, Matd& mat)
{
	mat.MakeZero();

	BodyLink* b = mModel->getBodyLink(index);
	int numDof = static_cast<int>(mModel->getNumDofs());

	for (int i = 0; i < numDof; ++i)
	{
		Dof* q1 = mModel->getDof(i);
		Dof* q2 = q;
		Vec3d com = b->getPrimitive()->getOffset();
		Vec4d homoCOM(com[0], com[1], com[2], 1);

		Vec4d cdot = b->getSecondDerivative(&homoCOM, q1, q2);

		mat[0][i] = cdot[0];
		mat[1][i] = cdot[1];
		mat[2][i] = cdot[2];

	}
}

void ForwardDynamics::approxBodyJW(int index, Matd& mat)
{
	mat.MakeZero();

	BodyLink* b = mModel->getBodyLink(index);
	Mat4d transform = b->getWorldTransform();
	Mat3d R = ExtractRotationFromTransform(transform);
	Mat3d transR = trans(R);

	int numDof = static_cast<int>(mModel->getNumDofs());
	for (int i = 0; i < numDof; ++i)
	{
		Dof* q = mModel->getDof(i);

		Mat4d identity = vl_I;
		Mat4d dWdq = b->approxRotFirstDerivative(&identity, q);
		Mat3d dRdq = ExtractRotationFromTransform(dWdq);
		Mat3d omegaSkewSymmetric = dRdq * transR;
		Vec3d omega = FromSkewSymmetric(omegaSkewSymmetric);
		omega = transR * omega;

		mat[0][i] = omega[0];
		mat[1][i] = omega[1];
		mat[2][i] = omega[2];

	}
}

void ForwardDynamics::approxBodyJWDot(int index, Matd& mat)
{
	mat.MakeZero();

	mat = (*(mBodyJWMatrices[index]) - *(mBodyLastJWMatrices[index])) / mTimeStep;
}

void ForwardDynamics::calculateBodyJWDot(int index, Matd& mat)
{
	mat.MakeZero();

	BodyLink* b = mModel->getBodyLink(index);
	Mat4d transform = b->getWorldTransform();
	Mat3d R = ExtractRotationFromTransform(transform);
	Mat3d transR = trans(R);


	Mat4d transformDot = calculateTransformDot(b);
	Mat3d RDot = ExtractRotationFromTransform(transformDot);
	Mat3d transRDot = trans(RDot);

	int numDof = static_cast<int>(mModel->getNumDofs());
	Mat4d identity = vl_I;
	for (int i = 0; i < numDof; ++i)
	{
		for (int j = 0; j < numDof; ++j)
		{
			Dof* q1 = mModel->getDof(i);
			Dof* q2 = mModel->getDof(j);
			if (b->dependsOnDof[q1->getModelID()] && b->dependsOnDof[q2->getModelID()])
			{
				Mat4d dWdq1q2 = b->getSecondDerivative(&identity, q1, q2);
				Mat4d dWdq1 = b->getFirstDerivative(&identity, q1);
				Mat4d dWdq2 = b->getFirstDerivative(&identity, q2);
				
				Mat3d dRdq1dq2 = ExtractRotationFromTransform(dWdq1q2);
				Mat3d dRdq1 = ExtractRotationFromTransform(dWdq1);
				Mat3d dRdq2 = ExtractRotationFromTransform(dWdq2);
				
				Mat3d sumSkewSymmetric1 = dRdq1 * transR;
				Mat3d sumSkewSymmetric2Part1 = dRdq1dq2 * transR;
				Mat3d sumSkewSymmetric2Part2 = dRdq1 * trans(dRdq2);
				Mat3d sumSkewSymmetric2 = sumSkewSymmetric2Part1 + sumSkewSymmetric2Part2;
				Vec3d sum = trans(dRdq2) * FromSkewSymmetric(sumSkewSymmetric1) + transR * FromSkewSymmetric(sumSkewSymmetric2);
				
			
				mat[0][i] += sum[0] * q2->getVelocity();
				mat[1][i] += sum[1] * q2->getVelocity();
				mat[2][i] += sum[2] * q2->getVelocity();
			}

		}

	}
}

Matd ForwardDynamics::CalculateJacobian(BodyLink* b, const Vec3d& localPos)
{
	int numDofs = mModel->getNumDofs();
	
	Matd ret(3, numDofs);
	ret.MakeZero();
	for (int i = 0; i < numDofs; ++i)
	{
		Dof* q = mModel->getDof(i);
		Vec4d homoPos(localPos[0], localPos[1], localPos[2], 1);
		Vec4d cDeriv = b->getFirstDerivative(&homoPos, q);

		ret[0][i] = cDeriv[0];
		ret[1][i] = cDeriv[1];
		ret[2][i] = cDeriv[2];
	}
	return ret;
}

void ForwardDynamics::calculateBodyJC(int index, Matd& mat)
{
	mat.MakeZero();

	BodyLink* b = mModel->getBodyLink(index);
	int numDof = static_cast<int>(mModel->getNumDofs());

	for (int i = 0; i < numDof; ++i)
	{
		Dof* q = mModel->getDof(i);
		Vec3d com = b->getPrimitive()->getOffset();
		Vec4d homoCOM(com[0], com[1], com[2], 1);
		Vec4d cDeriv = b->getFirstDerivative(&homoCOM, q);

		mat[0][i] = cDeriv[0];
		mat[1][i] = cDeriv[1];
		mat[2][i] = cDeriv[2];
	}

}

void ForwardDynamics::approxBodyJC(int index, Matd& mat)
{
	mat.MakeZero();

	BodyLink* b = mModel->getBodyLink(index);
	int numDof = static_cast<int>(mModel->getNumDofs());

	for (int i = 0; i < numDof; ++i)
	{
		Dof* q = mModel->getDof(i);
		Vec3d com = b->getPrimitive()->getOffset();
		Vec4d homoCOM(com[0], com[1], com[2], 1);
		Vec4d cDeriv = b->approxCOMFirstDerivative(&homoCOM, q);
		mat[0][i] = cDeriv[0];
		mat[1][i] = cDeriv[1];
		mat[2][i] = cDeriv[2];
	}
}


void ForwardDynamics::approxBodyJCDot(int index, Matd& mat)
{
	mat.MakeZero();

	mat = (*mBodyJCMatrices[index] - *mBodyLastJCMatrices[index]) / mTimeStep;
}


void ForwardDynamics::calculateBodyJCDot(int index, Matd& mat)
{
	mat.MakeZero();

	BodyLink* b = mModel->getBodyLink(index);
	int numDof = static_cast<int>(mModel->getNumDofs());

	for (int i = 0; i < numDof; ++i)
	{
		for (int j = 0; j < numDof; ++j)
		{
			Dof* q1 = mModel->getDof(i);
			Dof* q2 = mModel->getDof(j);
			Vec3d com = b->getPrimitive()->getOffset();
			Vec4d homoCOM(com[0], com[1], com[2], 1);

			double qDot = q2->getVelocity();
			Vec4d cdot = b->getSecondDerivative(&homoCOM, q1, q2);

			mat[0][i] += qDot * cdot[0];
			mat[1][i] += qDot * cdot[1];
			mat[2][i] += qDot * cdot[2];

		}
	}
}

void ForwardDynamics::clear()
{
	if (mMMatrix)
		delete mMMatrix;
	if (mCVector)
		delete mCVector;
	if (mTargetValueState)
		delete[] mTargetValueState;
	if (mTargetVelState)
		delete[] mTargetVelState;
	int num = static_cast<int>(mBodyMMatrices.size());
	for (int i = 0; i < num; ++i)
	{
		delete mBodyMMatrices[i];
	}
	mBodyMMatrices.clear();
	for (int i = 0; i < num; ++i)
	{
		delete mBodyCVectors[i];
	}
	mBodyCVectors.clear();
	for (int i = 0; i < num; ++i)
	{
		delete mBodyJCMatrices[i];
	}
	mBodyJCMatrices.clear();
	for (int i = 0; i < num; ++i)
	{
		delete mBodyLastJCMatrices[i];
	}
	mBodyLastJCMatrices.clear();
	for (int i = 0; i < num; ++i)
	{
		delete mBodyJCDotMatrices[i];
	}
	mBodyJCDotMatrices.clear();
	for (int i = 0; i < num; ++i)
	{
		delete mBodyJWMatrices[i];
	}
	mBodyJWMatrices.clear();
	for (int i = 0; i < num; ++i)
	{
		delete mBodyLastJWMatrices[i];
	}
	mBodyLastJWMatrices.clear();
	for (int i = 0; i < num; ++i)
	{
		delete mBodyJWDotMatrices[i];
	}
	mBodyJWDotMatrices.clear();
	for (int i = 0; i < num; ++i)
	{
		delete mBodyLastdTdQdot[i];
	}
	mBodyLastdTdQdot.clear();
}



Mat4d ForwardDynamics::calculateTransformDot(BodyLink* b)
{
	Mat4d ret(vl_0);
	Model* model = b->getModel();

	for(int j=0; j < model->getNumDofs(); j++){
		Dof* q = model->getDof(j);
		Mat4d dWdq = b->getFirstDerivative(&ret, q);
		double qDot = q->getVelocity();
		ret+=dWdq*qDot;
	}
	
	return ret;
}

Vecd ForwardDynamics::calculateForce()
{
	//mExternalForce = calculateExternalForce();
	mInternalForce = calculateInternalForce();
#ifndef FILE_IO_OFF
	outFile << "Internal Force : " << mInternalForce << endl;
	outFile << "External Force : " << mExternalForce << endl;
#endif
	return mInternalForce + mExternalForce;
}

void ForwardDynamics::SetExternalForce(const Vecd& force)
{
	mExternalForce = force;
}

Vecd ForwardDynamics::calculateExternalForce()
{
	Vec3d gravity(0, -9.8, 0);
	int numDofs = mModel->getNumDofs();
	Vecd generalizedForce(numDofs);
	generalizedForce = vl_0;
	int numLinks = mModel->getNumBodyLinks();

	for (int i = 0; i < numLinks; ++i)
	{
		Matd Jacobian = trans(*(mBodyJCMatrices[i]));
		double mass = mModel->getBodyLink(i)->getMass();
		generalizedForce += Jacobian * gravity * mass;
	}
	return generalizedForce;
}

Vecd ForwardDynamics::calculateInternalForce()
{
	int numDofs = mModel->getNumDofs();
	Vecd generalizedForce(numDofs);
	generalizedForce = vl_0;
	double targetValue = 0;	
	double targetVel = 0;
	double testValue[22];
	double testVel[22];
	for (int i = 6; i < numDofs; ++i)
	{
		if (mMode == TM_Planning)
		{
			generalizedForce[i] = 0;
			continue;
		}

		double currentValue = mModel->getDof(i)->getValue();
		double currentVel = mModel->getDof(i)->getVelocity();
		testValue[i] = currentValue;
		testVel[i] = currentVel;
		targetValue = mTargetValueState[i];
		targetVel = mTargetVelState[i];
		//double targetValue = mTarget->getDof(i)->getValue();
		//Dof* dof = mModel->getDof(i);
		//Joint* j = dof->getJoint();
		//BodyLink* b = j->getBodyOut();
		//double mass = b->getSubtreeMass();
		//ks *= mass;
		generalizedForce[i] = -ks * (currentValue - targetValue + mTimeStep * (currentVel)) - kd * (currentVel);// - targetVel);
	}
	generalizedForce[0] = -54.66 * 0.03f;

#ifndef FILE_IO_OFF
	outFile << "Current Dof: " << testValue[18] << ", "<< testValue[19] << ", "<< testValue[20] << ", "<< testValue[21] << "\n" ;

	outFile << "Target  Dof: " << mTargetValueState[18] << ", "<< mTargetValueState[19] << ", "<< mTargetValueState[20] << ", "<< mTargetValueState[21] << "\n";


	outFile << "Current Vel: " << testVel[18] << ", "<< testVel[19] << ", "<< testVel[20] << ", "<< testVel[21]<< "\n";

	outFile << "Target  Vel: " << mTargetVelState[18] << ", "<< mTargetVelState[19] << ", "<< mTargetVelState[20] << ", "<< mTargetVelState[21]<< "\n" ;
#endif


	return generalizedForce;

}

Vecd ForwardDynamics::CalculateGravityForce()
{
	Vec3d gravity(0, -9.8, 0);
	int numDofs = mModel->getNumDofs();
	Vecd generalizedForce(numDofs);
	generalizedForce = vl_0;
	int numLinks = mModel->getNumBodyLinks();

	for (int i = 0; i < numLinks; ++i)
	{
		calculateBodyJC(i, *(mBodyJCMatrices[i]));
		Matd Jacobian = trans(*(mBodyJCMatrices[i]));
		double mass = mModel->getBodyLink(i)->getMass();
		Vec3d weight = mass * gravity;
		generalizedForce += Jacobian * weight;
	}
	
	return generalizedForce;
}

double ForwardDynamics::approxBodydTdQ(int index, int ithDof)
{
	double currentT = calculateBodyKineticEnergy(index);
	Dof* q = mModel->getDof(ithDof);

	const double diff = 0.00001;
	double value = q->getValue();
	q->setValue(value + diff);
	double afterT = calculateBodyKineticEnergy(index);
	double ret = (afterT - currentT) / diff;
	q->setValue(value);
	return ret;
}
double ForwardDynamics::approxBodydTdQdot(int index, int ithDof)
{
	double currentT = calculateBodyKineticEnergy(index);
	Dof* q = mModel->getDof(ithDof);

	const double diff = 0.00001;
	double value = q->getVelocity();
	q->setVelocity(value + diff);
	double afterT = calculateBodyKineticEnergy(index);
	double ret = (afterT - currentT) / diff;
	q->setVelocity(value);
	return ret;
}
double ForwardDynamics::approxBodydTdQdotdt(int index, int ithDof)
{
	Dof* q = mModel->getDof(ithDof);

	double currentBodydTdQdot = approxBodydTdQdot(index, ithDof);
	double lastBodydTdQdot = (*mBodyLastdTdQdot[index])[ithDof];

	(*mBodyLastdTdQdot[index])[ithDof] = currentBodydTdQdot;
	return (currentBodydTdQdot - lastBodydTdQdot) / mTimeStep;
}

double ForwardDynamics::calculateBodyKineticEnergy(int index)
{
	BodyLink* b = mModel->getBodyLink(index);
	int numDofs = mModel->getNumDofs();
	double mass = b->getMass();
	Mat3d* inertia = b->getInertia();
	Vecd qDot = mModel->getQDot();

	Mat4d transform = b->getWorldTransform();
	Mat3d R = ExtractRotationFromTransform(transform);

	Matd JC(3, numDofs);
	Matd JW(3, numDofs);
	calculateBodyJC(index, JC);
	calculateBodyJW(index, JW);
	Vecd v = JC * qDot;
	Vecd w = JW * qDot;
	return 0.5 * (mass * dot(v, v) + dot(w, *inertia * w));
}

Vecd ForwardDynamics::ApproxdTdQdotdt()
{
	int numDofs = mModel->getNumDofs();
	int numLinks = mModel->getNumBodyLinks();
	Vecd ret(numDofs);
	ret = vl_0;

	for (int i = 0; i < numDofs; ++i)
	{
		for (int j = 0; j < numLinks; ++j)
		{
			ret[i] += approxBodydTdQdotdt(j, i);
		}
	}
	return ret;
}
Vecd ForwardDynamics::ApproxdTdQdot()
{
	int numDofs = mModel->getNumDofs();
	int numLinks = mModel->getNumBodyLinks();
	Vecd ret(numDofs);
	ret = vl_0;

	for (int i = 0; i < numDofs; ++i)
	{
		for (int j = 0; j < numLinks; ++j)
		{
			ret[i] += approxBodydTdQdot(j, i);
		}
	}
	return ret;
}
Vecd ForwardDynamics::ApproxdTdQ()
{
	int numDofs = mModel->getNumDofs();
	int numLinks = mModel->getNumBodyLinks();
	Vecd ret(numDofs);
	ret = vl_0;
	for (int i = 0; i < numDofs; ++i)
	{
		for (int j = 0; j < numLinks; ++j)
		{
			ret[i] += approxBodydTdQ(j, i);
		}
	}
	return ret;
}
double ForwardDynamics::CalculateKineticEnergy()
{
	double ret = 0;
	int numLinks = mModel->getNumBodyLinks();
	for (int i = 0; i < numLinks; ++i)
	{
		ret += calculateBodyKineticEnergy(i);
	}
	return ret;
}

Vec3d ForwardDynamics::ApproxLinearMomentum()
{
	int numBodies = mModel->getNumBodyLinks();
	int numDofs = mModel->getNumDofs();
	Vec3d ret = vl_0;
	for (int i = 0; i < numBodies; ++i)
	{
		BodyLink* b = mModel->getBodyLink(i);
		
		double mass = b->getMass();
		Vecd qDot = mModel->getQDot();

		Matd JC(3, numDofs);
		calculateBodyJC(i, JC);
		Vecd v = JC * qDot;
		Vecd momentum = mass * v;
		ret[0] += momentum[0];
		ret[1] += momentum[1];
		ret[2] += momentum[2];
	}
	return ret;
}
Vec3d ForwardDynamics::ApproxAngularMomentum()
{
	int numBodies = mModel->getNumBodyLinks();
	int numDofs = mModel->getNumDofs();
	Vec3d ret = vl_0;
	for (int i = 0; i < numBodies; ++i)
	{
		BodyLink* b = mModel->getBodyLink(i);

		Mat3d* inertia = b->getInertia();
		Vecd qDot = mModel->getQDot();

		Mat4d transform = b->getWorldTransform();
		Mat3d R = ExtractRotationFromTransform(transform);

		//Matd JW(3, numDofs);
		//calculateBodyJW(i, JW);
		Matd JW = *(mBodyJWMatrices[i]);
		Vecd w = JW * qDot;
		Vecd momentum = R * *inertia * w;
		ret[0] += momentum[0];
		ret[1] += momentum[1];
		ret[2] += momentum[2];
	}
	return ret;
}

Mat3d ExtractRotationFromTransform(Mat4d& m)
{
	Mat3d rotate;
	rotate[0] = Vec3d(m[0][0], m[0][1], m[0][2]);
	rotate[1] = Vec3d(m[1][0], m[1][1], m[1][2]);
	rotate[2] = Vec3d(m[2][0], m[2][1], m[2][2]);
	return rotate;
}

Vec3d ExtractTranslationFromTransform(Mat4d& m)
{
	Vec3d translation(m[0][3], m[1][3], m[2][3]);
	return translation;
}


double randDouble(double low, double high)
{
	double temp;

	/* swap low & high around if the user makes no sense */
	if (low > high)
	{
		temp = low;
		low = high;
		high = temp;
	}

	/* calculate the random number & return it */
	temp = (rand() / (static_cast<double>(RAND_MAX) + 1.0))
		* (high - low) + low;
	return temp;
}

DecoArchive& operator<< (DecoArchive& Ar, const ForwardDynamics& obj)
{
 	Ar << obj.mTimeStep;
	Ar << obj.mFrameCount;
	int numDofs = obj.mModel->getNumDofs();
	Ar << numDofs;
	for (int i = 0; i < numDofs; ++i)
	{
		Ar << obj.mTargetValueState[i];
		Ar << obj.mTargetVelState[i];
	}

	return Ar;
}
DecoArchive& operator>> (DecoArchive& Ar, ForwardDynamics& obj)
{
	Ar >> obj.mTimeStep;
	Ar >> obj.mFrameCount;

	int numDofs = 0;
	Ar >> numDofs;

	for (int i = 0; i < numDofs; ++i)
	{
		Ar >> obj.mTargetValueState[i];
		Ar >> obj.mTargetVelState[i];
	}
	return Ar;
}
